/*
 * CapstoneTests.c
 *
 * Created: 3/28/2013 3:21:47 PM
 *  Author: Alexander
 */ 


#include <avr/io.h>
#include "ADC.h"
#include "Servos.h"
#include "Gyro.h"
#include "SPI.h"
#include "Pressure_Sensor.h"
#include "U0_UART.h"
#include "Motor_Driver.h"
#include <util/delay.h>

extern FILE uart_stream;

int main(void)
{
	//Gyro_Init();
	//UARTInit();
	//Motor_Adjust();
	Motor_Driver_Only();
	return 1;
}

/**************************************************************************
* Method:		Motor_Adjust
* FullName:		Motor_Adjust
* Access:		public
* Returns:		void
* Qualifier:
* Description:	Allows the motor driver to be adjusted so that the water bags will be adjusted correctly
**************************************************************************/
void Motor_Adjust()
{
	PORTB |= (1<<PB7);				// Pull up resistors enable
	PORTE |=  ((1<<PE3) | (1<<PE2));
	MotorInit();
	//Down Button
	while(bit_is_set(PINB, PB7))
	{
		// Left Button
		if(bit_is_clear(PINE,PE3))
		MotorOnBack();
		//Right Button
		else if(bit_is_clear(PINE,PE2))
		MotorOnForward();
		else
		MotorOff();
	}
}


/**************************************************************************
* Method:		Motor_Driver_Only
* FullName:		Motor_Driver_Only
* Access:		public 
* Returns:		void
* Qualifier:	
* Description:	
**************************************************************************/
void Motor_Driver_Only()
{
	Motor_Adjust();
//	_delay_ms(30000);
	MotorInit();
	while(1)
	{
		MotorOnBack();
		_delay_ms(18000);
		MotorOff();
		_delay_ms(3000);
		MotorOnForward();
		_delay_ms(13000);
		MotorOff();
		_delay_ms(3000);
	}
}

void Gyro_Only()
{
	InitSPI();
	Gyro_Init();
	UARTInit();
	
	stderr = stdout = stdin = &uart_stream; /* Redirecting the standard I/O */

	AxesVector_ptr data = New_Vector();
	AxesVector_ptr prev = New_Vector();
	AxesVector_ptr zeroRate = Zero_Rate_Calc();
	Vector_Init(data);
	
	while(1)
	{
		Calculate_Angle(zeroRate, data, prev);		
		printf("\nX: %d \nY:%d \nZ:%d", data->AXIS_X, data->AXIS_Y, data->AXIS_Z);
		_delay_ms(500);
	}
}

/**************************************************************************
* Method:		Motor_Driver_And_Gyro
* FullName:		Motor_Driver_And_Gyro
* Access:		public 
* Returns:		void
* Qualifier:	
* Description:	
**************************************************************************/
void Motor_Driver_And_Gyro()
{
	InitSPI();
	MotorInit();
	Gyro_Init();
	AxesVector_ptr data = New_Vector();
	Vector_Init(data);
	DDRD |= (1<<PD3)|(1<<PD4);
	PORTD &= ~((1<<PD3)|(1<<PD4));
	
	while(1)
	{
		MotorOnForward();
		for(uint16_t i = 1600; i > 0; i--)
		{
			L3GD20_Read_Vectors_All(data);
			AngleCheck(data);
			_delay_ms(10);
		}	
		MotorOff();
		_delay_ms(20);
		MotorOnBack();
		for(uint16_t i = 1600; i > 0; i--)
		{
			L3GD20_Read_Vectors_All(data);
			AngleCheck(data);
			_delay_ms(10);
		}
	}
}

/**************************************************************************
* Method:		AngleCheck
* FullName:		AngleCheck
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	AxesVector_ptr angles
* Description:	
**************************************************************************/
void AngleCheck(AxesVector_ptr angles)
{
	int16_t y;
	y = angles->AXIS_Y;
	
	if(y < 10)
	{
		PORTD &= ~(1<<PD4);
		PIND |= (1<<PD3);
	}		
	else if(y > 10)
	{	
		PORTD &= ~(1<<PD3);
		PIND |= (1<<PD4);
	}		
}

/**************************************************************************
* Method:		Motor_Driver_And_Pressure_Sensor
* FullName:		Motor_Driver_And_Pressure_Sensor
* Access:		public 
* Returns:		void
* Qualifier:	
* Description:	
**************************************************************************/
void Motor_Driver_And_Pressure_Sensor()
{
	MotorInit();
	ADCSetup();
	while(1)	
	{
		MotorOnForward();
		for(uint16_t i = 1600; i > 0; i--)
		{
			ADCAquire();
			_delay_ms(10);
		}
		MotorOff();
		_delay_ms(20);
		MotorOnBack();
		for(uint16_t i = 1600; i > 0; i--)
		{
			ADCAquire();
			_delay_ms(10);
		}
		MotorOff();
		_delay_ms(20);
	}	
}